

#include "driverESC_I2C_Support.h"
#include "state.h"

void escBusDisableISR()
{
    IEC0CLR = _IEC0_I2C1MIE_MASK;
//    INTEnable				(INT_SOURCE_I2C_MASTER(I2C1), INT_DISABLED);
}

void escBusEnableISR()
{
    IEC0SET = _IEC0_I2C1MIE_MASK;
    //INTEnable				(INT_SOURCE_I2C_MASTER(I2C1), INT_ENABLED);
}


ESC_I2C_RESULT escI2CWait()
{
    unsigned int timeout = 0;
    while (IFS0bits.I2C1MIF == 0 && IFS0bits.I2C1BIF == 0 && timeout ++ < 1000000);

    if (IFS0bits.I2C1BIF || timeout >= 1000000)
    {
        portNOP();
        return BUSERROR;
    }

    IFS0CLR = _IFS0_I2C1MIF_MASK;

    return SUCCESS;
}

ESC_I2C_RESULT escI2CSendByte(unsigned char byte)
{
    I2C1TRN = byte;
    if (escI2CWait() != SUCCESS) return BUSERROR;

    if (I2C1STATbits.ACKSTAT == 0) return ACK;
    //if (I2CByteWasAcknowledged(I2C1)) return ACK;
    return NACK;
}


ESC_I2C_RESULT escI2CStart()
{
    IFS0CLR = _IFS0_I2C1MIF_MASK;
    I2C1CONSET = _I2C1CON_SEN_MASK;
    return escI2CWait();
}

ESC_I2C_RESULT escI2CRestart()
{
    IFS0CLR = _IFS0_I2C1MIF_MASK;
    I2C1CONSET = _I2C1CON_RSEN_MASK;
    return escI2CWait();
}

ESC_I2C_RESULT escI2CReadByte(unsigned char ack, unsigned char * target)
{
    if (I2C1STATbits.I2COV)
    {
        unsigned char blah = I2C1RCV;
        I2C1STATCLR = _I2C1STAT_I2COV_MASK;
    }
    IFS0CLR = _IFS0_I2C1MIF_MASK;
    
    I2C1CONSET = _I2C1CON_RCEN_MASK;
    if (escI2CWait() != SUCCESS) return BUSERROR;
    *target = I2C1RCV;
    
    if (ack)
        I2C1CONCLR = _I2C1CON_ACKDT_MASK;
    else
        I2C1CONSET = _I2C1CON_ACKDT_MASK;
    
    I2C1CONSET = _I2C1CON_ACKEN_MASK;
    return escI2CWait();
}

ESC_I2C_RESULT escI2CStop()
{
    //while ((I2C1CON & 0x1F) != 0x00);
    IFS0CLR = _IFS0_I2C1MIF_MASK;
    I2C1CONSET = _I2C1CON_PEN_MASK;
    return escI2CWait();
}

BOOL escI2CWriteBytes(BYTE motorId, BYTE reg, BYTE * buffer, int len, BYTE read)
{
    escI2CStart();
    unsigned char address = STATE.motors.address[motorId];
    escI2CSendByte(address);
    escI2CSendByte(reg);
    int i;
    for (i=0;i<len;i++) escI2CSendByte(buffer[i]);

    if (read)
    {
        escI2CRestart();
        escI2CSendByte(address|1);
    }
    else
    {
        escI2CStop();
    }
    return 1;
}
